﻿// 对方向观测值中的系统误差（旁折光）进行半参数建模

using MathNet.Numerics.LinearAlgebra;
using System;
using System.Collections.Generic;
using System.IO;
using System.Text;
using static AdjPlane.AdjCmn;

namespace AdjPlane
{
    using mat = Matrix<double>;
    using obs_dic = Dictionary<string, double>;
    using poi_dic = Dictionary<string, Point2D>;

    // 正则化参数计算方法
    public enum RegularParam
    {
        Lcurve,GCV,MSE
    }

    public struct SemiParam_t
    {
        public RegularParam regularParam;   // 正则化参数计算方法
        public int R_Order;     // 正则化矩阵时间序列差分阶数
    }

    public class SemiParam:Plane
    {
        const double MAX = 1e+20;

        //正则化矩阵   order:表示几阶差分
        static void GetReg_R(out mat R, int len, int order)
        {
            int i;
            mat G = Newmat(len - order, len);
            switch (order)
            {
                case 1:
                    for (i = 0; i < len - 1; i++)
                    {
                        G[i, i] = -1;
                        G[i, i + 1] = 1;
                    }
                    break;
                case 2:
                    for (i = 0; i < len - 2; i++)
                    {
                        G[i, i] = 0.5;
                        G[i, i + 1] = -1;
                        G[i, i + 2] = 0.5;
                    }
                    break;
                case 3:
                    for (i = 0; i < len - 3; i++)
                    {
                        G[i, i] = -1;
                        G[i, i + 1] = 1;
                        G[i, i + 2] = -1;
                        G[i, i + 3] = 1;
                    }
                    break;
                default:
                    G = Eye(len);
                    break;
            }
            R = G.Transpose() * G;
        }
        //正则化参数
        static double GetReg_alpha(mat R,mat A,mat P,mat L)
        {
            int i,len;
            double max,da,alpha,sigma2;
            
            max = 10;   //  正则化参数范围 0~max
            da = 0.1;   // 正则化参数的单位增量
            len =(int) (max / da);
            double[] lambda = new double[len];

            for (i = 0; i < len; i +=1) lambda[i] = i*da;

            int n = A.RowCount;
            var I = Eye(P.RowCount);

            double[] sn = new double[len];
            double[] vn = new double[len];
            double[] s2 = new double[len];

            double[] gcv = new double[len];
            double[] mse = new double[len];

            var Q = P.Inverse();
            //matprint(R);

            for (i = 0; i < len; i++)
            {
                if (i == 0)
                {
                    s2[0] = MAX;
                    gcv[0] = MAX;
                    mse[0] = MAX;
                    continue;
                }
                alpha = lambda[i];
                var M = (P + alpha * R).Inverse() * P;

                var diam = M.Diagonal();
                var N = A.Transpose() * P * A;
                N.Inverse();

                var Ns = A.Transpose() * P * (I - M) * A;
                var J = Ns.Inverse() * A.Transpose() * P * (I - M);
                var X = J * L;
                var S = M * (L - A * X);
                var V = A * X + S - L;
                sn[i] = (S.Transpose() * R * S)[0,0];
                vn[i] = (V.Transpose() * P * V)[0,0];
                s2[i] = sn[i] * sn[i] + vn[i] * vn[i];
                var H = M + (I - M) * A * J;
                sigma2 = vn[i] / (n - H.Trace());
                gcv[i] = vn[i] / Math.Pow((1 - H.Trace() / n),2);
                mse[i] = sigma2 * (J * Q * J.Transpose()).Trace() + ((J * S).Transpose() * (J * S)).Trace();
            }
            double[] curve1 = new double[len - 2];
            double[] curve2 = new double[len - 2];
            double[] curve3 = new double[len - 2];
            for (i = 1; i < len - 3; i++)
            {
                // 曲率计算公式1
                double dvs1 = (vn[i+1] - vn[i]) / (sn[i+1]-sn[i] );
                double dvs2 = (vn[i+2] - vn[i+1]) / (sn[i+2] - sn[i+1]);
                double difs = (sn[i + 1] - sn[i]) ;
                double ddvs = (dvs2 - dvs1) / (difs);
                curve1[i] = Math.Abs(ddvs) / Math.Sqrt(Math.Pow(1 + dvs1 * dvs1, 3));
                // 曲率计算公式2
                double dv = (vn[i + 1] - vn[i]) / da;
                double ds = (sn[i + 1] - sn[i]) / da;
                double ddv = (vn[i+2]+vn[i]-2*vn[i+1]) / (da*da);
                double dds = (sn[i+2]+sn[i]-2*sn[i+1]) / (da*da);
                curve2[i]=Math.Abs(ddv*ds-dv*dds)/Math.Pow(dv*dv+ds*ds,1.5);
                //曲率计算公式3
                curve3[i] =Math.Abs( -1 / ( Math.Pow(1 + lambda[i] * lambda[i], 1.5) * ds));
            }
            int id_Lclose = (int)Min(s2)[1];
            int id_Lcorner1 = (int)Max(curve1)[1];
            int id_Lcorner2 = (int)Max(curve2)[1];
            int id_Lcorner3 = (int)Max(curve3)[1];
            
            int id_gcv = (int)Min(gcv)[1];
            int id_mse = (int)Min(mse)[1];
            return lambda[id_gcv];
        }

        // 只对部分观测值进行半参数建模（第1~s个观测值）
        // ref [16]
        static double GetReg_alpha_partial(RegularParam method,mat R, mat A, mat P, mat L,int n,int s)
        {
            double max = 10;   //  正则化参数范围 0~max
            double da = 0.1;   // 正则化参数的单位增量
            int len = (int)(max / da);
            double[] lambda = new double[len];

            for (int i = 0; i < len; i += 1) lambda[i] = i * da;

            double[] sn = new double[len];
            double[] vn = new double[len];
            double[] s2 = new double[len];

            double[] gcv = new double[len];
            double[] mse = new double[len];

            var Q = P.Inverse();
            var Ps = P.SubMatrix(0, s, 0, s);
            var Pc = P.SubMatrix(s, n - s, s, n - s);
            var As = A.SubMatrix(0, s, 0, A.ColumnCount);
            var Ac= A.SubMatrix(s, n-s, 0, A.ColumnCount);

            double alpha, sigma2;
            for (int  i = 0; i < len; i++)
            {
                if (i == 0)
                {
                    s2[0] = MAX;
                    gcv[0] = MAX;
                    mse[0] = MAX;
                    continue;
                }
                alpha = lambda[i];
                var M = (Ps + alpha * R).Inverse() * Ps;
                var M_ = M.DiagonalStack(Newmat(n - s, n - s));
                //var Ns = A.Transpose() * P * (Eye(n) - M_) * A;
                var Ns = (Ac.Transpose() * Pc * Ac) + (As.Transpose() * Ps * (Eye(s) - M) * As);
                var J = Ns.Inverse() * A.Transpose() * P * (Eye(n) - M_);
                var X = J * L;
                
                var S_ = M_ * (L - A * X);
                var V = A * X + S_ - L;
                var S = S_.SubMatrix(0, s, 0, 1);
                sn[i] = (S.Transpose() * R * S)[0, 0];
                vn[i] = (V.Transpose() * P * V)[0, 0];
                s2[i] = sn[i] * sn[i] + vn[i] * vn[i];
                
                var H = M_ + (Eye(n) - M_) * A * J;
                sigma2 = vn[i] / (n - H.Trace());
                gcv[i] = vn[i] / Math.Pow((1 - H.Trace() / n), 2);
                mse[i] = sigma2 * (J * Q * J.Transpose()).Trace() + ((J * S_).Transpose() * (J * S_)).Trace();
            }
            double[] curve1 = new double[len - 2];
            double[] curve2 = new double[len - 2];
            double[] curve3 = new double[len - 2];
            for (int i = 1; i < len - 3; i++)
            {
                // 曲率计算公式1
                double dvs1 = (vn[i + 1] - vn[i]) / (sn[i + 1] - sn[i]);
                double dvs2 = (vn[i + 2] - vn[i + 1]) / (sn[i + 2] - sn[i + 1]);
                double difs = (sn[i + 1] - sn[i]);
                double ddvs = (dvs2 - dvs1) / (difs);
                curve1[i] = Math.Abs(ddvs) / Math.Sqrt(Math.Pow(1 + dvs1 * dvs1, 3));
                // 曲率计算公式2
                double dv = (vn[i + 1] - vn[i]) / da;
                double ds = (sn[i + 1] - sn[i]) / da;
                double ddv = (vn[i + 2] + vn[i] - 2 * vn[i + 1]) / (da * da);
                double dds = (sn[i + 2] + sn[i] - 2 * sn[i + 1]) / (da * da);
                curve2[i] = Math.Abs(ddv * ds - dv * dds) / Math.Pow(dv * dv + ds * ds, 1.5);
                //曲率计算公式3
                curve3[i] = Math.Abs(-1 / (Math.Pow(1 + lambda[i] * lambda[i], 1.5) * ds));
            }
            int id_Lclose = (int)Min(s2)[1];
            int id_Lcorner1 = (int)Max(curve1)[1];
            int id_Lcorner2 = (int)Max(curve2)[1];
            int id_Lcorner3 = (int)Max(curve3)[1];

            int id_gcv = (int)Min(gcv)[1];
            int id_mse = (int)Min(mse)[1];
            if (method == RegularParam.Lcurve)
                return lambda[id_Lcorner3];
            else if (method == RegularParam.GCV)
                return lambda[id_gcv];
            else if (method== RegularParam.MSE)
                return lambda[id_mse];
            else
                return max;
        }

        // 半参数模型的平面网平差
        // ref [14],[15]
        void Adjust(Proc_t popt,File_t fopt)
        {
            int Sign = SideSign * popt.SideEdge; //区分隧道侧壁方向观测值
            int iter = 0;
            mat Xprev=Newmat(t,1);
            Result = new poi_dic(Approx);//近似坐标作为迭代的初值
            mat AL1, AL2,LL1,LL2,R,M,M1,M2;//AL1,AL2表示，沿侧壁的方向观测系数阵，其余方向系数阵
            double reg_a;  //正则化参数
            mat ROC;

            AL1 = Newmat(nl, t);AL2 = Newmat(nl, t);
            LL1 = Newmat(nl, 1);LL2 = Newmat(nl, 1);

            P = Newmat(n, n); QX = Newmat(t, t); 
            N1 = Newmat(t, t); W1 = Newmat(t, 1);
            N2 = Newmat(t, t); W2 = Newmat(t, 1);
            N = Newmat(t, t); W = Newmat(t, 1);
            X = Newmat(t, 1); V1 = Newmat(nl, 1); V2 = Newmat(ns, 1);

            //方差因子初值
            M0 = alpha; qv = M0 * M0;
            qvl = qv; qvs = qv;

            //定初权
            P1 = Newmat(nl, nl); P2 = Newmat(ns, ns);
            P1 = GetP1(nl, M0); P2 = GetP2(ns, M0, popt.DisWgt);
            P = P1.DiagonalStack(P2);

            //建立误差方程矩阵
            A1 = Newmat(nl, t); L1 = Newmat(nl, 1);
            A2 = Newmat(ns, t);  L2 = Newmat(ns, 1);
            Orientation_Angle(out obs_dic DXJ);//计算定向角
            if (Sign == 0)
                GetFX1(DXJ, ref A1, ref L1);
            else
            {
                GetFX2(DXJ, out AL1, out LL1, out AL2, out LL2);
                A1 = AL1.Stack(AL2);
                L1 = LL1.Stack(LL2);
            }
            GetJL1(ref A2, ref L2);
            A = A1.Stack(A2);
            L = L1.Stack(L2);
            P = P1.DiagonalStack(P2);

            int R_order = popt.SemiParamOpt.R_Order;
            RegularParam alpha_method = popt.SemiParamOpt.regularParam;
            if (Sign == 1)
            {
                int nL1 = AL1.RowCount, nL2 = AL2.RowCount;
                GetReg_R(out R, nL1, R_order);
                //reg_a = GetReg_alpha(R, AL1, PP1, LL1);
                reg_a = GetReg_alpha_partial(alpha_method, R, A, P, L, n, nL1);
                mat PL1 = P1.SubMatrix(0, nL1, 0, nL1);
                M1 =(PL1+reg_a*R).Inverse()*PL1;
                M2 = Newmat(ns + nL2, ns + nL2);
            }
            else
            {
                GetReg_R(out R, nl, R_order);
                //reg_a = GetReg_alpha(R, A1, P1, L1);
                reg_a = GetReg_alpha_partial(alpha_method, R, A, P, L, n, nl);
                M1 = (P1 + reg_a * R).Inverse() * P1;
                M2 = Newmat(ns, ns);
            }

            M = M1.DiagonalStack(M2);
            mat I = Eye(n);
            var Ns = A.Transpose() * P * (I - M) * A;
            var Ws = A.Transpose() * P * (I - M) * L;
            X = Ns.Inverse() * Ws;

            if (popt.WgtDef != Weight.Tradition)
            {
                V1 = A1 * X - L1;
                V2 = A2 * X - L2;
                PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
            }

            mat S;
            while (true)
            {
                if(popt.WgtDef == Weight.hmt2)
                {
                    N1 = A1.Transpose() * P1*(Eye(nl)-M1) * A1;
                    N2 = A2.Transpose() * P2 * A2;
                    Ns = N1 + N2;
                    double r1 = nl - (Ns.Inverse() * N1).Trace();
                    double r2 = ns - (Ns.Inverse() * N2).Trace();
                    int hh=0;
                    while (true)
                    {
                        qvl = (V1.Transpose() * P1 * V1)[0, 0] / r1;
                        qvs = (V2.Transpose() * P2 * V2)[0, 0] / r2;
                        P1 = qvl / qvl * P1;  //权阵迭代
                        P2 = qvl / qvs * P2;
                        double bias = Math.Abs(qvl / qvs - 1);
                        if (bias < popt.Eps_VCE || hh == popt.IterMax_VCE) //结束方差分量估计迭代
                            break;
                        VCE_t cov = new VCE_t
                        {
                            count = iter_VCE,
                            ql = qvl,
                            qs = qvs
                        };
                        iter_VCE++;
                        cov_res.Add(cov);//存储方差分量估计的定权结果
                        hh++;
                    }
                }
                A = A1.Stack(A2);
                L = L1.Stack(L2);
                Ns = A.Transpose() * P * (I - M) * A;
                Ws = A.Transpose() * P * (I - M) * L;
                X = Ns.Inverse() * Ws;
                if (popt.WgtDef != Weight.Tradition)
                {
                    V1 = A1 * X - L1;
                    V2 = A2 * X - L2;
                    PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                    PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
                }
                foreach (KeyValuePair<string, int> dic in ukpIndex)
                {
                    Result[dic.Key].X += X[2 * dic.Value, 0] / 1000;
                    Result[dic.Key].Y += X[2 * dic.Value + 1, 0] / 1000;
                }
                if (Sign == 1)
                    S = M1 * (LL1 - AL1 * X);
                else
                    S = M1 * (L1 - A1 * X);
                double eps, epsMax = 0;
                for (int j = 0; j < t; j++)
                {
                    eps = Math.Abs(X[j, 0] - Xprev[j, 0]);
                    if (eps > epsMax) 
                        epsMax = eps;
                }
                if (epsMax < popt.Eps_X|| iter == popt.IterMax_X)  //判断是否结束近似坐标迭代
                {
                    VCE_t w = new VCE_t
                    {
                        count = iter_VCE,
                        ql = qvl,
                        qs = qvs
                    };
                    cov_res.Add(w);
                    break;
                }
                iter++;
                //如果没达到迭代阈值，重新建立误差方程；坐标迭代后，误差方程矩阵也跟着改变
                Orientation_Angle(out DXJ);//计算定向角
                if (Sign == 0)
                    GetFX1(DXJ, ref A1, ref L1);
                else
                {
                    GetFX2(DXJ, out AL1, out LL1, out AL2, out LL2);
                    A1 = AL1.Stack(AL2);
                    L1 = LL1.Stack(LL2);
                }
                GetJL1(ref A2, ref L2);
                Xprev = X;
            }

            //协因数和多余观测分量
            var G = Ns.Inverse() * A.Transpose() * P * (I - M);
            QX = G * P.Inverse() * G.Transpose();
            var H1 = M + (I - M) * A * Ns.Inverse() * A.Transpose() * P * (I - M);
            ROC = I - H1;

            int i = 0; int i1 = 0, i2 = 0, side;
            Q1 = P1.Inverse();
            foreach (KeyValuePair<string,obs_dic> sta in obsL)
            {
                foreach (KeyValuePair<string,double> obs in sta.Value)
                {
                    side =Math.Abs( FindSide(sta.Key, obs.Key));
                    int ids = FindStaName(sta.Key);
                    int ido = FindObsName(ids,obs.Key);
                    double v=0;
                    if (Sign == 1)
                    {
                        if (side == 2)
                        {
                            v = (AL1 * X)[i1, 0] + S[i1, 0] - LL1[i1, 0];
                            i1++;
                        }
                        else
                        {
                            v = (AL2 * X)[i2, 0] - LL2[i2, 0];
                            i2++;
                        }
                    }
                    else
                        v = (A1 * X)[i, 0] + S[i,0] - L1[i, 0];
                    V1[i, 0] = v;
                    AllObs[ids].data[ido].vl = v;
                    AllObs[ids].data[ido].qL = Q1[i, i];
                    AllObs[ids].data[ido].rocL = ROC[i, i];
                    i++;
                }
            }
            i = 0; Q2 = P2.Inverse();
            foreach (KeyValuePair<string, obs_dic> sta in obsS)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    int ids = FindStaName(sta.Key);
                    int ido = FindObsName(ids, obs.Key);
                    double v = (A2 * X)[i, 0] - L2[i, 0];
                    V2[i, 0] = v;
                    AllObs[ids].data[ido].vs = v;
                    AllObs[ids].data[ido].qS = Q2[i, i];
                    AllObs[ids].data[ido].rocS = ROC[nl + i, nl + i];
                    i++;
                }
            }
            V = V1.Stack(V2);
            PVV = (V.Transpose() * P * V)[0,0];
            PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
            PVV2= (V2.Transpose() * P2 * V2)[0, 0];

            var H = M + (I - M) * A * Ns.Inverse() * A.Transpose() * P * (I - M);
            t = (int)H.Trace();
            this.r = n-t;
            double r = n - H.Trace();
            sigma =Math.Sqrt( PVV / r);
            if (WgtDef == Weight.Tradition) fai = M0;
            else fai = sigma; //验后单位权中误差

            reg_alpha = reg_a;
            Matfprint(S.ToArray(), fopt.SysErrFile);
            Print("regular param : " + reg_a);
            Print("iter count :" + iter);
            Print("tr(ROC)=" + ROC.Trace());
            Print("r = " + r);
        }

        //给输出文件起名字
        new void Name(Proc_t popt,ref File_t fopt)
        {
            string sep = "\\";
            string dir = Path.GetDirectoryName(fopt.infile);
            string p = Path.GetFileNameWithoutExtension(fopt.infile);
            string name;
            RegularParam reg = popt.SemiParamOpt.regularParam;
            string ext;
            if (reg == RegularParam.Lcurve)
            {
                name = p + "_SemiParam_Lcurve.ou2";
                ext = "_S_Lcurve.txt";
            }
            else if (reg == RegularParam.GCV)
            {
                name = p + "_SemiParam_GCV.ou2";
                ext = "_S_GCV.txt";
            }
            else if (reg == RegularParam.MSE)
            {
                name = p + "_SemiParam_MSE.ou2";
                ext = "_S_MSE.txt";
            }
            else
            {
                name = p + "_SemiParam.ou2";
                ext = "_S.txt";
            }
            fopt.SysErrFile = dir+sep+p+ext;
            fopt.oufile = dir+sep+name;
        }

        // 半参数估计平面网平差
        public new void PlaneAdj(Proc_t popt, ref File_t fopt)
        {
            global_popt = popt;
            ReadFile(fopt.infile,popt);//读取
            Name(popt, ref fopt);
            Approximate(popt,ref fopt);//概算 
            Adjust(popt,fopt);//平差
            Output(popt,fopt.oufile);//输出
        }
    }
}
